#include "IDW.h"
#include "esp_log.h"
#include <stdio.h>
/**
 * @brief 双线性  放大16倍
 * 
 * @param p 目标地址
 * @param rows 目标高
 * @param cols 目标宽
 * @param x 坐标x
 * @param y 坐标y
 * @param f 目标值
 */
static inline void set_point(int16_t *p, uint16_t rows, uint16_t cols, int16_t x, int16_t y, float f)
{
    p[y * cols + x] = f;
}

static float bilinear_get_point(const float *p, uint8_t rows, uint8_t cols, int8_t x, int8_t y)
{
    if (x >= cols)
        x = cols - 1;
    if (y >= rows)
        y = rows - 1;
    return p[y * cols + x];
}

// p is a 4-point 2x2 array of the rows & columns left/right/above/below
// p是一个由左/右/上/下的行和列组成的4点2x2数组
static float bilinear_interpolate(const float p[], float x, float y)
{
    float xx = p[1] * x + p[0] * (1.f - x);
    float xy = p[3] * x + p[2] * (1.f - x);
    return xy * y + xx * (1.f - y);
}

// src is rows*cols and dest is a 4-point array passed in already allocated!
// src 是 rows*cols 并且 dest 是一个已经分配的 4 点数组！
static void bilinear_get_adjacents_2d(const float *src, float *dest, uint8_t rows, uint8_t cols, int8_t x, int8_t y)
{
    dest[0] = bilinear_get_point(src, rows, cols, x + 0, y + 0);
    dest[1] = bilinear_get_point(src, rows, cols, x + 1, y + 0);
    dest[2] = bilinear_get_point(src, rows, cols, x + 0, y + 1);
    dest[3] = bilinear_get_point(src, rows, cols, x + 1, y + 1);
}

/**
 * @brief 二次插值
 * 
 * @param src 输入图像地址
 * @param src_rows 输入图像宽
 * @param src_cols 输入图像高
 * @param dest 输出图像地址(放大16倍的整形)
 * @param dest_rows 输出图像宽
 * @param dest_cols 输出图像高
 * @param upScaleFactor 比例因子
 */
void idwBilinear(const float *src, uint8_t src_rows, uint8_t src_cols,
                 int16_t *dest, uint16_t dest_rows, uint16_t dest_cols, uint8_t upScaleFactor)
{

    const float mu = 1.f / upScaleFactor;
    float adj_2d[4]; // 存储相邻矩阵

    for (uint16_t y_idx = 0; y_idx < dest_rows; y_idx++)
    {
        for (uint16_t x_idx = 0; x_idx < dest_cols; x_idx++)
        {
            float x = x_idx * mu;
            float y = y_idx * mu;
            bilinear_get_adjacents_2d(src, adj_2d, src_rows, src_cols, x, y);

            float frac_x = x - (int)x; // 我们只需要点之间的~delta~
            float frac_y = y - (int)y; // 我们只需要点之间的~delta~
            float out = bilinear_interpolate(adj_2d, frac_x, frac_y);
            set_point(dest, dest_rows, dest_cols, x_idx, y_idx, out);
        }
    }
}

//==============================================================================
//
//==============================================================================

/**
 * @brief 老双线性插值
 * 
 * @param src 输入图像地址
 * @param src_rows 输入图像宽
 * @param src_cols 输入图像高
 * @param steps 比例因子
 * @param pHdImage 输出图像地址
 */
void idwOldInterpolate(int16_t *src, uint8_t src_rows, uint8_t src_cols, uint8_t steps, int16_t *pHdImage)
{
    uint32_t OutIdx = 0;
    uint16_t outRows = ((src_rows - 1) * steps);
    // 水平插值 32 -> 279 (288)
    for (uint16_t row = 0; row < src_cols; row++)
    {
        for (uint16_t col = 0; col < (src_rows - 1); col++)
        {
            uint16_t ImageIdx = row * src_rows + col;
            int16_t tempStart = src[ImageIdx];
            int16_t tempEnd = src[ImageIdx + 1];
            
            for (uint16_t step = 0; step < steps; step++)
            {
                uint32_t Idx = (OutIdx + col) * steps + step;
                pHdImage[Idx] = (tempStart * (steps - step) + tempEnd * step) / steps;
            }
        }
        OutIdx += outRows;
    }

    // 竖直插值 24 -> 207 (216)
    for (uint16_t col = 0; col < outRows; col++)
    {
        for (uint16_t row = 0; row < src_cols; row++)
        {
            int16_t tempStart = pHdImage[row * steps * outRows + col];
            int16_t tempEnd = pHdImage[(row + 1) * steps * outRows + col];

            for (uint16_t step = 1; step < steps; step++)
            {
                uint32_t Idx = (row * steps + step) * outRows + col;
                pHdImage[Idx] = tempStart * (steps - step) / steps + tempEnd * step / steps;
            }
        }
    }
}
